#include "ardrone_driver.h"
#include "teleop_twist.h"
#include "video.h"
#include <signal.h>

////////////////////////////////////////////////////////////////////////////////
// class ARDroneDriver
////////////////////////////////////////////////////////////////////////////////
using namespace std;
ARDroneDriver::ARDroneDriver()
    : image_transport(node_handle)
{
    inited = false;
    last_frame_id = -1;
    last_navdata_id = -1;
    cmd_vel_sub = node_handle.subscribe("cmd_vel", 1, &cmdVelCallback);
	takeoff_sub = node_handle.subscribe("ardrone/takeoff", 1, &takeoffCallback);
	reset_sub = node_handle.subscribe("ardrone/reset", 1, &resetCallback);
	land_sub = node_handle.subscribe("ardrone/land", 1, &landCallback);
	image_pub = image_transport.advertiseCamera("ardrone/image_raw", 10);
    hori_pub = image_transport.advertiseCamera("ardrone/front/image_raw", 10);
	vert_pub = image_transport.advertiseCamera("ardrone/bottom/image_raw", 10);
    navdata_pub = node_handle.advertise<ardrone_autonomy::Navdata>("ardrone/navdata", 25);
    imu_pub = node_handle.advertise<sensor_msgs::Imu>("ardrone/imu", 25);
    mag_pub = node_handle.advertise<geometry_msgs::Vector3Stamped>("ardrone/mag", 25);
	toggleCam_service = node_handle.advertiseService("ardrone/togglecam", toggleCamCallback);
    setCamChannel_service = node_handle.advertiseService("ardrone/setcamchannel",setCamChannelCallback );
	setLedAnimation_service = node_handle.advertiseService("ardrone/setledanimation", setLedAnimationCallback);
    //flatTrim_service = node_handle.advertiseService("ardrone/flattrim", flatTrimCallback);
    flattrim_sub = node_handle.subscribe("/ardrone/flattrim", 1, &flattrimCallback);
     posecmd_vel_sub = node_handle.subscribe("/pose/cmd_vel", 1, &posecmdVelCallback);
	 yawcmd_vel_sub = node_handle.subscribe("/pose/yaw_cmd_vel", 1, &yawcmdVelCallback);

     teleopcmd_vel_sub = node_handle.subscribe("/teleop/cmd_vel", 1, &teleopcmdVelCallback);

    /*
        To be honest, I am not sure why advertising a service using class members should be this complicated!
        One day, I should learn what is exactly happenning here. /M
    */
    imuReCalib_service = node_handle.advertiseService<std_srvs::Empty::Request, std_srvs::Empty::Response>
            ("ardrone/imu_recalib", boost::bind(&ARDroneDriver::imuReCalibCallback, this, _1, _2));

//	setEnemyColor_service = node_handle.advertiseService("/ardrone/setenemycolor", setEnemyColorCallback);
//	setHullType_service = node_handle.advertiseService("/ardrone/sethulltype", setHullTypeCallback);

    droneFrameId = (ros::param::get("~drone_frame_id", droneFrameId)) ? droneFrameId : "ardrone_base";
    droneFrameBase = droneFrameId + "_link";
    droneFrameIMU = droneFrameId + "_imu";
    droneFrameFrontCam = droneFrameId + "_frontcam";
    droneFrameBottomCam = droneFrameId + "_bottomcam";

    drone_root_frame = (ros::param::get("~root_frame", drone_root_frame)) ? drone_root_frame : ROOT_FRAME_BASE;
    drone_root_frame = drone_root_frame % ROOT_FRAME_NUM;
    ROS_INFO("Root Frame is: %d", drone_root_frame);

    // Fill constant parts of IMU Message
    // If no rosparam is set then the default value of 0.0 will be assigned to all covariance values

    for (int i = 0; i < 9; i++)
    {
        imu_msg.linear_acceleration_covariance[i] = 0.0;
        imu_msg.angular_velocity_covariance[i] = 0.0;
        imu_msg.orientation_covariance[i] = 0.0;
    }
    readCovParams("~cov/imu_la", imu_msg.linear_acceleration_covariance);
    readCovParams("~cov/imu_av", imu_msg.angular_velocity_covariance);
    readCovParams("~cov/imu_or", imu_msg.orientation_covariance);

    // Caliberation
    max_num_samples = 50;
    do_caliberation = (ros::param::get("~do_imu_caliberation", do_caliberation)) ? do_caliberation : false;
    if (do_caliberation) {
        resetCaliberation();
        ROS_WARN("Automatic IMU Caliberation is active.");
    }

    // Camera Info Manager
    cinfo_hori_ = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/front"), "ardrone_front");
    cinfo_vert_ = new camera_info_manager::CameraInfoManager(ros::NodeHandle("ardrone/bottom"), "ardrone_bottom");

    // TF Stuff


    // Front Cam to Base
    // TODO: Precise values for Drone1 & Drone2
    tf_base_front = tf::StampedTransform(
                tf::Transform(
                    tf::createQuaternionFromRPY(-90.0 * _DEG2RAD, 0.0, -90.0 * _DEG2RAD),
                    tf::Vector3(0.21, 0.0, 0.0)),
                ros::Time::now(), droneFrameBase, droneFrameFrontCam
                );


    // Bottom Cam to Base (Bad Assumption: No translation from IMU and Base)
    // TODO: This should be different from Drone 1 & 2.
    tf_base_bottom = tf::StampedTransform(
                tf::Transform(
                    tf::createQuaternionFromRPY(180.0 * _DEG2RAD, 0.0, 90.0 * _DEG2RAD),
                    tf::Vector3(0.0, -0.02, 0.0)),
                ros::Time::now(), droneFrameBase, droneFrameBottomCam
                );

    // Changing the root for TF if needed
    if (drone_root_frame == ROOT_FRAME_FRONT)
    {
        tf_base_front.setData(tf_base_front.inverse());
        tf_base_front.child_frame_id_.swap(tf_base_front.frame_id_);
    }
    else if (drone_root_frame == ROOT_FRAME_BOTTOM)
    {
        tf_base_bottom.setData(tf_base_bottom.inverse());
        tf_base_bottom.child_frame_id_.swap(tf_base_bottom.frame_id_);
    }   

}

ARDroneDriver::~ARDroneDriver()
{
    delete cinfo_hori_;
    delete cinfo_vert_;
}

void ARDroneDriver::run()
{
    // TODO: 50Hz made navdata unstable, I think it is a locking issue.
    ros::Rate loop_rate(200);
    ros::Time startTime = ros::Time::now();
    static int freq_dev = 0;
	while (node_handle.ok())
	{
        if (!inited) // Give the Drone 5s of free time to finish init phase
        {
            if (((ros::Time::now() - startTime).toSec()) > 5.0)
            {
                inited = true;

                // Send the configuration to the drone
                configureDrone();

                vp_os_mutex_lock(&navdata_lock);
                ROS_INFO("Successfully connected to '%s' (AR-Drone %d.0 - Firmware: %s) - Battery(\%): %d",
                         ardrone_control_config.ardrone_name,
                         (IS_ARDRONE1) ? 1 : 2,
                         ardrone_control_config.num_version_soft,
                         shared_navdata.vbat_flying_percentage);
                vp_os_mutex_unlock(&navdata_lock);
                if (ardrone_control_config.num_version_soft[0] == '0')
                {
                    ROS_WARN("The AR-Drone has a suspicious Firmware number. It usually means the network link is unreliable.");
                }
            }
        } else {
            if (current_frame_id != last_frame_id)
            {
                publish_video();                                
                last_frame_id = current_frame_id;
            }
            if (current_navdata_id != last_navdata_id)
            {
                publish_navdata();
                last_navdata_id = current_navdata_id;
            }
            if (freq_dev == 0) //publish_tf();

            freq_dev = (freq_dev + 1) % 5; // ~10Hz TF publish
        }
        ros::spinOnce();
		loop_rate.sleep();
	}
    printf("ROS loop terminated ... \n");
}

void ARDroneDriver::configureDrone()
{
    // This function will send the custom configuration to the drone
    // It doesn't work if sent during the SDK (which runs before the configuration profiles on the drone are setup)
    #undef ARDRONE_CONFIG_KEY_IMM_a10
    #undef ARDRONE_CONFIG_KEY_REF_a10
    #undef ARDRONE_CONFIG_KEY_STR_a10
    #undef LOAD_PARAM_STR
    #undef LOAD_PARAM_NUM

    #define SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT)                                                          \
    {                                                                                                      \
        if(ardrone_application_default_config.NAME!=DEFAULT)                                               \
        {                                                                                                  \
            ROS_INFO("SEND: "#CATEGORY"/"#NAME" = %f (DEFAULT = %f)", (float)ardrone_application_default_config.NAME, (float)DEFAULT);           \
            ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, &ardrone_application_default_config.NAME, NULL);    \
        }                                                                                                  \
    }

    #define SEND_PARAM_STR(NAME,CATEGORY,DEFAULT)                                                          \
    {                                                                                                      \
        if(0!=strcmp(ardrone_application_default_config.NAME,DEFAULT))                                     \
        {                                                                                                  \
            ROS_INFO("SEND: "#CATEGORY"/"#NAME" = %s (DEFAULT = %s)", ardrone_application_default_config.NAME, DEFAULT);           \
            ARDRONE_TOOL_CONFIGURATION_ADDEVENT (NAME, ardrone_application_default_config.NAME, NULL);     \
        }                                                                                                  \
    }

    // firstly we send the CAT_COMMON parameters, for example outdoor, as these settings can affect where the other parameters are stored
    #define ARDRONE_CONFIG_KEY_REF_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) //do nothing for reference-only parameters
    #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
    #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY==CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }

    #include <config_keys.h> // include the parameter definitions, which will be replaced by the above

    #undef ARDRONE_CONFIG_KEY_IMM_a10
    #undef ARDRONE_CONFIG_KEY_STR_a10

    // then we send the rest of the parameters. The problem is if we send euler_angle_max (for example) before sending outdoor, it will get written to the wrong parameter
    // (indoor_ not outdoor_euler_angle_max) and then will be overwritten by the default when changing state from indoor to outdoor, so we need to send common parameters first.
    #define ARDRONE_CONFIG_KEY_IMM_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_NUM(NAME,CATEGORY,DEFAULT) } // parameters under the custom key are for control of application/user/session, we don't want to change these!
    #define ARDRONE_CONFIG_KEY_STR_a10(KEY, NAME, INI_TYPE, C_TYPE, C_TYPE_PTR, RW, RW_CUSTOM, DEFAULT, CALLBACK, CATEGORY) { if(0!=strcmp(KEY,"custom") && CATEGORY!=CAT_COMMON && ((RW & K_WRITE) != 0 || (RW_CUSTOM & K_WRITE) != 0)) SEND_PARAM_STR(NAME,CATEGORY,DEFAULT) }

    #include <config_keys.h> // include the parameter definitions, which will be replaced by the above

    #undef SEND_PARAM_NUM
    #undef SEND_PARAM_STR
    #undef ARDRONE_CONFIG_KEY_IMM_a10
    #undef ARDRONE_CONFIG_KEY_REF_a10
    #undef ARDRONE_CONFIG_KEY_STR_a10
}

void ARDroneDriver::resetCaliberation()
{
    caliberated = false;
    acc_samples.clear();
    gyro_samples.clear();
    vel_samples.clear();
    for (int i = 0; i < 3; i++)
    {
        acc_bias[i] = 0.0;
        vel_bias[i] = 0.0;
        gyro_bias[i] = 0.0;
        acc_samples.push_back(std::vector<double> ());
        gyro_samples.push_back(std::vector<double> ());
        vel_samples.push_back(std::vector<double> ());
    }
}

double ARDroneDriver::calcAverage(std::vector<double> &vec)
{
    double ret = 0.0;
    for (unsigned int i = 0; i < vec.size(); i++)
    {
        ret += vec.at(i);
    }
    return (ret / vec.size());
}

bool ARDroneDriver::readCovParams(std::string param_name, boost::array<double, 9> &cov_array)
{
    XmlRpc::XmlRpcValue cov_list;
    std::stringstream str_stream;
    if (ros::param::get(param_name, cov_list))
    {
         if (cov_list.getType() != XmlRpc::XmlRpcValue::TypeArray)
         {
             ROS_WARN("Covariance values for %s is not a list", param_name.c_str());
             return false;
         }

         if (cov_list.size() != 9)
         {
             ROS_WARN("Covariance list size for %s is not of size 9 (Size: %d)", param_name.c_str(), cov_list.size());
             return false;
         }
         str_stream << param_name << " set to [";
         for (int32_t i = 0; i < cov_list.size(); i++)
         {
             ROS_ASSERT(cov_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble);
             cov_array[i] = static_cast<double> (cov_list[i]);
             str_stream << cov_array[i] << ((i < 8) ? ", " : "");
         }
         str_stream << "]";
         ROS_INFO(str_stream.str().c_str());
         return true;

    }
    else
    {
        ROS_INFO("No values found for `%s` in parameters, set all to zero.", param_name.c_str());
        return false;
    }

}

double ARDroneDriver::getRosParam(char* param, double defaultVal)
{
    std::string name(param);
    double res, ret;
    ret =  (ros::param::get(name, res)) ? res : defaultVal;
    ROS_INFO("SET %-30s: %4.2f", param, ret);
    return ret;
}

void ARDroneDriver::publish_video()
{
    if (
            (image_pub.getNumSubscribers() == 0) &&
            (hori_pub.getNumSubscribers() == 0) &&
            (vert_pub.getNumSubscribers() == 0)
       ) return;

    // Camera Info (NO PIP)

    sensor_msgs::CameraInfo cinfo_msg_hori = cinfo_hori_->getCameraInfo();
    sensor_msgs::CameraInfo cinfo_msg_vert = cinfo_vert_->getCameraInfo();

    cinfo_msg_hori.header.frame_id = droneFrameFrontCam;
    cinfo_msg_vert.header.frame_id = droneFrameBottomCam;

    if (IS_ARDRONE1)
    {
        /*
        * Information on buffer and image sizes.
        * Buffer is always in QVGA size, however for different Camera Modes
        * The picture and PIP sizes are different.
        * 
        * image_raw and buffer are always 320x240. In order to preserve backward compatibilty image_raw remains
        * always as before. Two new set of topics are added for two new cameras : /ardrone/front/xxx and /ardrone/bottom/xxx
        * 
        * In Camera State 0 front image relays the buffer  and image_raw and bottom image are not updated.
        * 
        * In Camera State 1 bottom image is a 174x144 crop of the buffer. The front image is not updated
        * 
        * In Camera State 2 bottom image is a PIP cut of size (87x72) from buffer.
        * The bottom image is a (320-87)x(240) cut of the buffer.
        * 
        * In Camera State 3 front image is a PIP cut of size (58x42) from buffer.
        * The bottom image is a (174-58)x144 crop of the buffer. 
        */
        sensor_msgs::Image image_msg;
        sensor_msgs::Image::_data_type::iterator _it;

        image_msg.header.stamp = ros::Time::now();
	cinfo_msg_hori.header.stamp = image_msg.header.stamp;
	cinfo_msg_vert.header.stamp = image_msg.header.stamp;
        if ((cam_state == ZAP_CHANNEL_HORI) || (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT))
        {
            image_msg.header.frame_id = droneFrameFrontCam;
        }
        else if ((cam_state == ZAP_CHANNEL_VERT) || (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI))
        {
            image_msg.header.frame_id = droneFrameBottomCam;
        }
        else
        {
            ROS_WARN_ONCE("Something is wrong with camera channel config.");
        }

        image_msg.width = D1_STREAM_WIDTH;
        image_msg.height = D1_STREAM_HEIGHT;
        image_msg.encoding = "rgb8";
        image_msg.is_bigendian = false;
        image_msg.step = D1_STREAM_WIDTH*3;
        image_msg.data.resize(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3);

        vp_os_mutex_lock(&video_lock);
        std::copy(buffer, buffer+(D1_STREAM_WIDTH*D1_STREAM_HEIGHT*3), image_msg.data.begin());
        vp_os_mutex_unlock(&video_lock);

        if (cam_state == ZAP_CHANNEL_HORI)
        {
            /*
            * Horizontal camera is activated, only /ardrone/front/ is being updated 
            */
            cinfo_msg_hori.width = D1_STREAM_WIDTH;
            cinfo_msg_hori.height = D1_STREAM_HEIGHT;

            image_pub.publish(image_msg, cinfo_msg_hori);
            hori_pub.publish(image_msg, cinfo_msg_hori);
        }
        else if (cam_state == ZAP_CHANNEL_VERT)
        {
            /*
            * Vertical camera is activated, only /ardrone/bottom/ is being updated 
            */
            image_msg.width = D1_VERTSTREAM_WIDTH;
            image_msg.height = D1_VERTSTREAM_HEIGHT;
            image_msg.encoding = "rgb8";
            image_msg.is_bigendian = false;
            image_msg.step = D1_VERTSTREAM_WIDTH*3;
            image_msg.data.clear();
            image_msg.data.resize(D1_VERTSTREAM_WIDTH*D1_VERTSTREAM_HEIGHT*3);
            _it = image_msg.data.begin();
            vp_os_mutex_lock(&video_lock);
            for (int row = 0; row < D1_VERTSTREAM_HEIGHT ; row++)
            {
                int _b = row * D1_STREAM_WIDTH * 3;
                int _e = _b + image_msg.step;
                _it = std::copy(buffer + _b, buffer + _e, _it);
            }
            vp_os_mutex_unlock(&video_lock);

            cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH;
            cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
            image_pub.publish(image_msg, cinfo_msg_vert);
            vert_pub.publish(image_msg, cinfo_msg_vert);
        }
        else if (cam_state == ZAP_CHANNEL_LARGE_HORI_SMALL_VERT)
        {
            /*
            * The Picture in Picture is activated with vertical camera inside the horizontal
            * camera. Both /ardrone/front and /ardrone/bottom is being updated
            */

            // Front (Cropping the first 88 columns)
            image_msg.width = D1_STREAM_WIDTH  - D1_MODE2_PIP_WIDTH;
            image_msg.height = D1_STREAM_HEIGHT;
            image_msg.encoding = "rgb8";
            image_msg.is_bigendian = false;
            image_msg.step = (D1_STREAM_WIDTH-D1_MODE2_PIP_WIDTH)*3;
            image_msg.data.clear();
            image_msg.data.resize((D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH)*D1_STREAM_HEIGHT*3);
            _it = image_msg.data.begin();
            vp_os_mutex_lock(&video_lock);
            for (int row = 0; row < D1_STREAM_HEIGHT; row++)
            {
                int _b = (row * D1_STREAM_WIDTH * 3) + (D1_MODE2_PIP_WIDTH * 3);
                int _e = _b + image_msg.step;
                _it = std::copy(buffer + _b, buffer + _e, _it);
            }
            vp_os_mutex_unlock(&video_lock);

            cinfo_msg_hori.width = D1_STREAM_WIDTH - D1_MODE2_PIP_WIDTH;
            cinfo_msg_hori.height = D1_STREAM_HEIGHT;
            hori_pub.publish(image_msg, cinfo_msg_hori);

            //Bottom
            image_msg.width = D1_MODE2_PIP_WIDTH;
            image_msg.height = D1_MODE2_PIP_HEIGHT;
            image_msg.encoding = "rgb8";
            image_msg.is_bigendian = false;
            image_msg.step = D1_MODE2_PIP_WIDTH * 3;
            image_msg.data.clear();
            image_msg.data.resize(D1_MODE2_PIP_WIDTH * D1_MODE2_PIP_HEIGHT * 3);
            _it = image_msg.data.begin();
            vp_os_mutex_lock(&video_lock);
            for (int row = 0; row < D1_MODE2_PIP_HEIGHT; row++)
            {
                int _b = row * D1_STREAM_WIDTH * 3;
                int _e = _b + image_msg.step;
                _it = std::copy(buffer + _b, buffer + _e, _it);
            }
            vp_os_mutex_unlock(&video_lock);

            cinfo_msg_vert.width = D1_MODE2_PIP_WIDTH;
            cinfo_msg_vert.height = D1_MODE2_PIP_HEIGHT;
            vert_pub.publish(image_msg, cinfo_msg_vert);
        }
        else if (cam_state == ZAP_CHANNEL_LARGE_VERT_SMALL_HORI)
        {
            /*
            * The Picture in Picture is activated with horizontal camera inside the vertical
            * camera. Both /ardrone/front and /ardrone/bottom is being updated
            */

            // Bottom  (Cropping the first 58 columns)
            image_msg.width = D1_VERTSTREAM_WIDTH   - D1_MODE3_PIP_WIDTH;
            image_msg.height = D1_VERTSTREAM_HEIGHT;
            image_msg.encoding = "rgb8";
            image_msg.is_bigendian = false;
            image_msg.step = (D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH)*3;
            image_msg.data.clear();
            image_msg.data.resize((D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH)* D1_VERTSTREAM_HEIGHT*3);
            _it = image_msg.data.begin();
            for (int row = 0; row < D1_VERTSTREAM_HEIGHT; row++)
            {
                int _b = (row * (D1_STREAM_WIDTH * 3)) + (D1_MODE3_PIP_WIDTH * 3);
                int _e = _b + image_msg.step;
                vp_os_mutex_lock(&video_lock);
                _it = std::copy(buffer + _b, buffer + _e, _it);
                vp_os_mutex_unlock(&video_lock);
            }

            cinfo_msg_vert.width = D1_VERTSTREAM_WIDTH - D1_MODE3_PIP_WIDTH;
            cinfo_msg_vert.height = D1_VERTSTREAM_HEIGHT;
            vert_pub.publish(image_msg, cinfo_msg_vert);

            //Front
            image_msg.width = D1_MODE3_PIP_WIDTH;
            image_msg.height = D1_MODE3_PIP_HEIGHT;
            image_msg.encoding = "rgb8";
            image_msg.is_bigendian = false;
            image_msg.step = D1_MODE3_PIP_WIDTH * 3;
            image_msg.data.clear();
            image_msg.data.resize(D1_MODE3_PIP_WIDTH * D1_MODE3_PIP_HEIGHT * 3);
            _it = image_msg.data.begin();
            vp_os_mutex_lock(&video_lock);
            for (int row = 0; row < D1_MODE3_PIP_HEIGHT; row++)
            {
                int _b = row * D1_STREAM_WIDTH * 3;
                int _e = _b + image_msg.step;
                _it = std::copy(buffer + _b, buffer + _e, _it);
            }vp_os_mutex_unlock(&video_lock);

            cinfo_msg_hori.width = D1_MODE3_PIP_WIDTH;
            cinfo_msg_hori.height = D1_MODE3_PIP_HEIGHT;
            hori_pub.publish(image_msg, cinfo_msg_hori);
        }
    }
    
    /**
     * For Drone 2 w/ SDK2. Both camera streams are 360p.
     * No 720p support for now.
     * SDK 2.0 Does not support PIP.
     */
    if (IS_ARDRONE2)
    {
        sensor_msgs::Image image_msg;        
        sensor_msgs::Image::_data_type::iterator _it;

        image_msg.header.stamp = ros::Time::now();
        cinfo_msg_hori.header.stamp = image_msg.header.stamp; 
        cinfo_msg_vert.header.stamp = image_msg.header.stamp; 

        if (cam_state == ZAP_CHANNEL_HORI)
        {
            image_msg.header.frame_id = droneFrameFrontCam;
        }
        else if (cam_state == ZAP_CHANNEL_VERT)
        {
            image_msg.header.frame_id = droneFrameBottomCam;
        }
        else
        {
            ROS_WARN_ONCE("Something is wrong with camera channel config.");
        }

        image_msg.width = D2_STREAM_WIDTH;
        image_msg.height = D2_STREAM_HEIGHT;
        image_msg.encoding = "rgb8";
        image_msg.is_bigendian = false;
        image_msg.step = D2_STREAM_WIDTH*3;
        image_msg.data.resize(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3);
        vp_os_mutex_lock(&video_lock);
        std::copy(buffer, buffer+(D2_STREAM_WIDTH*D2_STREAM_HEIGHT*3), image_msg.data.begin());
        vp_os_mutex_unlock(&video_lock);
        // We only put the width and height in here.


        if (cam_state == ZAP_CHANNEL_HORI)
        {
            /*
            * Horizontal camera is activated, only /ardrone/front/ is being updated 
            */
            cinfo_msg_hori.width = D2_STREAM_WIDTH;
            cinfo_msg_hori.height = D2_STREAM_HEIGHT;
            image_pub.publish(image_msg, cinfo_msg_hori); // /ardrone
            hori_pub.publish(image_msg, cinfo_msg_hori);
        }
        else if (cam_state == ZAP_CHANNEL_VERT)
        {
            /*
            * Vertical camera is activated, only /ardrone/bottom/ is being updated 
            */
            cinfo_msg_vert.width = D2_STREAM_WIDTH;
            cinfo_msg_vert.height = D2_STREAM_HEIGHT;
            image_pub.publish(image_msg, cinfo_msg_vert); // /ardrone
            vert_pub.publish(image_msg, cinfo_msg_vert);
        }
    }
	
}

void ARDroneDriver::publish_navdata()
{
    // Thread safe copy of interesting Navdata data
    vp_os_mutex_lock(&navdata_lock);
    navdata_detect = shared_navdata_detect;
    navdata_phys = shared_navdata_phys;
    navdata = shared_navdata;
    arnavtime = shared_arnavtime;
    if (IS_ARDRONE2)
    { // This is neccessary
        navdata_pressure = shared_navdata_pressure;
        navdata_magneto = shared_navdata_magneto;
        navdata_wind = shared_navdata_wind;
    }
    vp_os_mutex_unlock(&navdata_lock);


    if ((do_caliberation) && (!caliberated))
    {
        acc_samples[0].push_back(navdata_phys.phys_accs[ACC_X]);
        acc_samples[1].push_back(navdata_phys.phys_accs[ACC_Y]);
        acc_samples[2].push_back(navdata_phys.phys_accs[ACC_Z]);
        gyro_samples[0].push_back(navdata_phys.phys_gyros[GYRO_X]);
        gyro_samples[1].push_back(navdata_phys.phys_gyros[GYRO_Y]);
        gyro_samples[2].push_back(navdata_phys.phys_gyros[GYRO_Z]);
        vel_samples[0].push_back(navdata.vx);
        vel_samples[1].push_back(navdata.vy);
        vel_samples[2].push_back(navdata.vz);
        if (acc_samples[0].size() == max_num_samples)
        {
            for (int j = 0; j < 3; j++)
            {
                acc_bias[j] = calcAverage(acc_samples[j]);
                gyro_bias[j] = calcAverage(gyro_samples[j]);
                vel_bias[j] = calcAverage(vel_samples[j]);
            }
            ROS_INFO("Bias in linear acceleration (mg): [%4.4lf, %4.4lf, %4.4lf]", acc_bias[0], acc_bias[1], acc_bias[2]);
            ROS_INFO("Bias in angular velocity (deg/s): [%4.4lf, %4.4lf, %4.4lf]", gyro_bias[0], gyro_bias[1], gyro_bias[2]);
            ROS_INFO("Bias in linear velocity (mm/s): [%4.4lf, %4.4lf, %4.4lf]", vel_bias[0], vel_bias[1], vel_bias[2]);
            ROS_INFO("Above values (except z-axis accel) will be substracted from actual IMU data in `navdata` and `imu` topic.");
            ROS_INFO("This feature can be disabled using `do_imu_caliberation` parameter. Recaliberate using `imu_recalib` service.");
            caliberated = true;
        }
    }
    if ((do_caliberation) && (caliberated))
    {
        for (int j = 0; j < 3; j++)
        {
            if (j != 2) navdata_phys.phys_accs[j] -= acc_bias[j];
            navdata_phys.phys_gyros[j] -= gyro_bias[j];
        }
        navdata.vx -= vel_bias[0];
        navdata.vy -= vel_bias[1];
        navdata.vz -= vel_bias[2];

    }
    if ((navdata_pub.getNumSubscribers() == 0) && (imu_pub.getNumSubscribers() == 0) && (mag_pub.getNumSubscribers() == 0))
        return; // why bother, no one is listening.
    const ros::Time _now = ros::Time::now();

	ardrone_autonomy::Navdata msg;

    msg.header.stamp = _now;
    msg.header.frame_id = droneFrameBase;
	msg.batteryPercent = navdata.vbat_flying_percentage;
    msg.state = (navdata.ctrl_state >> 16);
    
	// positive means counterclockwise rotation around axis
	msg.rotX = navdata.phi / 1000.0; // tilt left/right
	msg.rotY = navdata.theta / 1000.0; // tilt forward/backward
	msg.rotZ = navdata.psi / 1000.0; // orientation

    /* ==============================================================
     For some reason, sonar measure is different for each state.
     For instance, while flying the sensor measures actual 1m as 0.76cm
     On the other hand, it measures acurate height with hand carrying.
     Therefore, publishing height data with ardrone's states.
     This offset comes from the ground truth measurement.
     ================================================================ */ 
    
    
	if(msg.state==2)
	{
		//std::cout<<"Not flying"<<std::endl;	
		msg.altd = navdata.altitude/1000.0; // m
	}
	// 3,4,7,8 states
	else msg.altd = (float)(navdata.altitude/1000.0+HEIGHT_OFFSET); 

	//msg.altd = navdata.altitude/1000.0;
	//cout<<"state ="<<msg.state<<endl;
	//msg.vx = navdata.vx/1000; // m/sec
	//msg.vy = navdata.vy/1000; // m/sec
	//msg.vz = navdata.vz/1000; // m/sec
	msg.vx = veldata.x/1000;
	msg.vy = veldata.y/1000;
	msg.vz = veldata.z/1000;
	
	msg.tm = arnavtime.time;
	msg.ax = navdata_phys.phys_accs[ACC_X] / 1000.0; // g
	msg.ay = navdata_phys.phys_accs[ACC_Y] / 1000.0; // g
	msg.az = navdata_phys.phys_accs[ACC_Z] / 1000.0; // g

	// New stuff

    if (IS_ARDRONE2)
    {
        msg.magX = (int32_t)navdata_magneto.mx;
        msg.magY = (int32_t)navdata_magneto.my;
        msg.magZ = (int32_t)navdata_magneto.mz;

        msg.pressure = navdata_pressure.Pression_meas; // typo in the SDK!
        msg.temp = navdata_pressure.Temperature_meas;

        msg.wind_speed = navdata_wind.wind_speed;
        msg.wind_angle = navdata_wind.wind_angle;
        msg.wind_comp_angle = navdata_wind.wind_compensation_phi;
    }
    else
    {
        msg.magX = msg.magY = msg.magZ = 0;
        msg.pressure = 0.0;
        msg.temp = 0.0;
        msg.wind_speed = 0.0;
        msg.wind_angle = 0.0;
        msg.wind_comp_angle = 0.0;
    }

	// Tag Detection
	msg.tags_count = navdata_detect.nb_detected;
    for (int i = 0; i < navdata_detect.nb_detected; i++)
	{
		/*
		 * The tags_type is in raw format. In order to extract the information 
		 * macros from ardrone_api.h is needed.
		 *
		 * #define DETECTION_MAKE_TYPE(source,tag) ( ((source)<<16) | (tag) )
		 * #define DETECTION_EXTRACT_SOURCE(type)  ( ((type)>>16) & 0x0FF )
		 * #define DETECTION_EXTRACT_TAG(type)     ( (type) & 0x0FF )
		 * 
		 * Please also note that the xc, yc, width and height are in [0,1000] range
		 * and must get converted back based on image resolution.
		 */
		msg.tags_type.push_back(navdata_detect.type[i]);
		msg.tags_xc.push_back(navdata_detect.xc[i]);
		msg.tags_yc.push_back(navdata_detect.yc[i]);
		msg.tags_width.push_back(navdata_detect.width[i]);
		msg.tags_height.push_back(navdata_detect.height[i]);
		msg.tags_orientation.push_back(navdata_detect.orientation_angle[i]);
		msg.tags_distance.push_back(navdata_detect.dist[i]);
	}

    navdata_pub.publish(msg);

    /* IMU */
    imu_msg.header.frame_id = droneFrameBase;
    imu_msg.header.stamp = _now;

    // IMU - Linear Acc
    imu_msg.linear_acceleration.x = msg.ax * 9.8;
    imu_msg.linear_acceleration.y = msg.ay * 9.8;
    imu_msg.linear_acceleration.z = msg.az * 9.8;

    // IMU - Rotation Matrix
    btQuaternion q;
    q.setEulerZYX(msg.rotZ * _DEG2RAD, msg.rotY * _DEG2RAD, msg.rotX * _DEG2RAD);
    tf::quaternionTFToMsg(q, imu_msg.orientation);

    // IMU - Gyro (Gyro is being sent in deg/sec)
    // TODO: Should Gyro be added to Navdata?
    imu_msg.angular_velocity.x = navdata_phys.phys_gyros[GYRO_X] * DEG_TO_RAD;
    imu_msg.angular_velocity.y = -navdata_phys.phys_gyros[GYRO_Y] * DEG_TO_RAD;
    imu_msg.angular_velocity.z = -navdata_phys.phys_gyros[GYRO_Z] * DEG_TO_RAD;

    mag_msg.header.frame_id = droneFrameBase;
    mag_msg.header.stamp = _now;
    const float mag_normalizer = sqrt( msg.magX * msg.magX + msg.magY * msg.magY + msg.magZ * msg.magZ );

    // TODO: Check if it is really needed that magnetometer message includes normalized value
    if (fabs(mag_normalizer) > 1e-9f)
    {
        mag_msg.vector.x = msg.magX / mag_normalizer;
        mag_msg.vector.y = msg.magY / mag_normalizer;
        mag_msg.vector.z = msg.magZ / mag_normalizer;
        mag_pub.publish(mag_msg);
    }
    else
    {
        //ROS_WARN_THROTTLE(1, "There is something wrong with the magnetometer readings (Magnitude is extremely small).");
    }

    navdata_pub.publish(msg);
    imu_pub.publish(imu_msg);
}

void ARDroneDriver::publish_tf()
{
    tf_base_front.stamp_ = ros::Time::now();
    tf_base_bottom.stamp_ = ros::Time::now();
    tf_broad.sendTransform(tf_base_front);
    tf_broad.sendTransform(tf_base_bottom);
}

bool ARDroneDriver::imuReCalibCallback(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
{
    if (!do_caliberation)
    {
        ROS_WARN("Automatic IMU Caliberation is not active. Activate first using `do_imu_caliberation` parameter");
        return false;
    }
    else
    {
        ROS_WARN("Recaliberating IMU, please do not move the drone for a couple of seconds.");
        resetCaliberation();
        return true;
    }
}

void controlCHandler (int signal)
{
    ros::shutdown();
    should_exit = 1;    
}
////////////////////////////////////////////////////////////////////////////////
// custom_main
////////////////////////////////////////////////////////////////////////////////

//extern "C" int custom_main(int argc, char** argv)
int main(int argc, char** argv)
{        
    C_RESULT res = C_FAIL;
    char * drone_ip_address = NULL;

        // We need to implement our own Signal handler instead of ROS to shutdown
        // the SDK threads correctly.

        ros::init(argc, argv, "ardrone_driver", ros::init_options::NoSigintHandler);
        
        signal (SIGABRT, &controlCHandler);
        signal (SIGTERM, &controlCHandler);
        signal (SIGINT, &controlCHandler);

    // Now to setup the drone and communication channels
    // We do this here because calling ardrone_tool_main uses an old
    // function initialization and is no longer recommended by parrot
    // I've based this section off the ControlEngine's initialization
    // routine (distributed with ARDrone SDK 2.0 Examples) as well as
    // the ardrone_tool_main function

    // Parse command line for
    // Backward compatibility with `-ip` command line argument
    argc--; argv++;
    while( argc && *argv[0] == '-' )
    {
        if( !strcmp(*argv, "-ip") && ( argc > 1 ) )
        {
            drone_ip_address = *(argv+1);
            printf("Using custom ip address %s\n",drone_ip_address);
            argc--; argv++;
        }
        argc--; argv++;
    }

    // Configure wifi
    vp_com_wifi_config_t *config = (vp_com_wifi_config_t*)wifi_config();

    if(config)
    {

        vp_os_memset( &wifi_ardrone_ip[0], 0, ARDRONE_IPADDRESS_SIZE );

        // TODO: Check if IP is valid
        if(drone_ip_address)
        {
          printf("===================+> %s\n", drone_ip_address);
          strncpy( &wifi_ardrone_ip[0], drone_ip_address, ARDRONE_IPADDRESS_SIZE - 1);
        }
        else
        {
          printf("===================+> %s\n", config->server);
          strncpy( &wifi_ardrone_ip[0], config->server, ARDRONE_IPADDRESS_SIZE - 1);
        }
}

    while (-1 == getDroneVersion (".", wifi_ardrone_ip, &ardroneVersion))
    {
        printf ("Getting AR.Drone version ...\n");
        vp_os_delay (250);
    }

    // Setup communication channels
    res = ardrone_tool_setup_com( NULL );
    if( FAILED(res) )
    {
        PRINT("Wifi initialization failed. It means either:\n");
        PRINT("\t* you're not root (it's mandatory because you can set up wifi connection only as root)\n");
        PRINT("\t* wifi device is not present (on your pc or on your card)\n");
        PRINT("\t* you set the wrong name for wifi interface (for example rausb0 instead of wlan0) \n");
        PRINT("\t* ap is not up (reboot card or remove wifi usb dongle)\n");
        PRINT("\t* wifi device has no antenna\n");
    }
    else
    {
        // setup the application and user profiles for the driver

        char* appname = DRIVER_APPNAME;
        char* usrname = DRIVER_USERNAME;
        ardrone_gen_appid (appname, "2.0", app_id, app_name, APPLI_NAME_SIZE);
        ardrone_gen_usrid (usrname, usr_id, usr_name, USER_NAME_SIZE);

        // and finally initialize everything!
        // this will then call our sdk, which then starts the ::run() method of this file as an ardrone client thread

        res = ardrone_tool_init(wifi_ardrone_ip, strlen(wifi_ardrone_ip), NULL, app_name, usr_name, NULL, NULL, MAX_FLIGHT_STORING_SIZE, NULL);

        while( SUCCEED(res) && ardrone_tool_exit() == FALSE )
        {
            res = ardrone_tool_update();
        }
        res = ardrone_tool_shutdown();
    }
    return SUCCEED(res) ? 0 : -1;
}


